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,__l Abstract. We present a general and modular algorithmic framework 

^— ( for path planning of robots. Our framework combines geometric meth- 

("^ ods for exact and complete analysis of low-dimensional configuration 

Cn spaces, together with practical, considerably simpler sampling-based ap- 

1— H proaches that are appropriate for higher dimensions. In order to facil- 

, ^ itate the transfer of advanced geometric algorithms into practical use, 

' we suggest taking samples that are entire low- dimensional manifolds of 

*/^ the configuration space that capture the connectivity of the configuration 

^_^ space much better than isolated point samples. Geometric algorithms for 

O analysis of low-dimensional manifolds then provide powerful primitive 
operations. The modular design of the framework enables independent 

\,J optimization of each modular component. Indeed, we have developed, im- 

(/2 plemented and optimized a primitive operation for complete and exact 

O combinatorial analysis of a certain set of manifolds, using arrangements 

of curves of rational functions and concepts of generic programming. This 

I in turn enabled us to implement our framework for the concrete case of a 

^ polygonal robot translating and rotating amidst polygonal obstacles. We 

^V^ demonstrate that the integration of several carefully engineered compo- 

f^ nents leads to significant speedup over the popular PRM sampling-based 

00 algorithm, which represents the more simplistic approach that is preva- 

^^ lent in practice. We foresee possible extensions of our framework to solv- 

[^^ ing high-dimensional problems beyond motion planning. 

o 

^— I 1 Introduction 

. 5^ Motion planning is a fundamental research topic in robotics with applications in 

S^ diverse domains such as graphical animation, surgical planning, computational 

^ biology and computer games. For a general overview of the subject and its appli- 

cations see |10I21I23] . In its basic form, the motion-planning problem is to find 
a collision-free path for a robot or a moving object i? in a workspace cluttered 
with static obstacles. The spatial pose of R, or the configuration of R, is uniquely 
defined by some set of parameters, the degrees of freedom (rfo/s) of R. The set 
of all robot configurations C is termed the configuration space of the robot, and 
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decomposes into the disjoint sets of free and forbidden configurations, namely 
Cfioc and Cforb, respectively. Thus, it is common to rephrase the motion-planning 
problem as the problem of moving R from a start configuration Qs to a target 
configuration qt in a path that is fully contained within Cftcc- 

Analytic solutions to the general motion planning problem: The 

motion-planning problem is computationally hard with respect to the number 
of dofs {2^, yet much research has been devoted to solving the general problem 
and its various instances using geometric, algebraic and combinatorial tools. The 
configuration-space formalism was introduced by Lozano-Perez [5S] in the early 
1980's. Schwartz and Sharir proposed the first general algorithm for solving the 
motion planning problem, with running time that is doubly-exponential in the 
number of dofs [SU]. Singly exponential-time algorithms have followed |4I8I9J . 
but are generally considered too complicated to be implemented in practice. 

Solutions to lovir-dimensional instances of the problem: Although 
the general motion-planning problem cannot be efficiently solved analytically, 
more efficient algorithms have been proposed for various low-dimensional in- 
stances [H], such as translating a polygonal or polyhedral robot |ll25j . and 
translation with rotation of a polygonal robot in the plane |i3lf4129j . For a survey 
of related approaches see [3T]. Moreover, considerable advances in robust imple- 
mentation of computational geometry algorithms in recent years have led to a set 
of implemented tools that are of interest in this context. Minkowski sums, which 
allow representation of the configuration space of a translating robot, have ro- 
bust and exact planar and 3-dimensional implementations |f2ll3l34] . Likewise, 
implementations of planar arrangementtrl for curves [331 C.30], are essential 
components in |30j . 

Sampling-based approaches to motion planning: The sampling-based 
approach to motion-planning has extended the applicability of motion planning 
algorithms beyond the restricted subset of problems that can be solved efficiently 
by exact algorithms |fOI23| . Sampling-based motion planning algorithms, such 
as Probabihstic Roadmaps (PRM) TH], Expansive Space Trees (EST) [TH] and 
Rapidly-exploring Random Trees (RRT) [22], as well as their many variants, 
aim to capture the connectivity of Chee in a graph data structure, via random 
sampling of robot configurations. This can be done either in a multi-query set- 
ting, to efficiently answer multiple queries for the same scenario, as in the PRM 
algorithm, or in a single-query setting, as in the RRT and EST algorithms. For a 
general survey on the field see [in|. Importantly, the PRM and RRT algorithms 
were both shown to be probabilistically complete |I7)f9|20] . that is, they are 
guaranteed to find a valid solution, if one exists. However, the required running 
time for finding such a solution cannot be computed for new queries at run-time, 
and the proper usage of sampling-based approaches may still be considered some- 
what of an art. Moreover, sampling-based methods are also considered sensitive 
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to tight passages in the configuration space, due to the high-probabihty of miss- 
ing the passage. 

Hybrid methods for motion-planning: Few hybrid methods attempt to 
combine both deterministic and probabiUstic planning strategies. Hirsch and 
Halperin [15] studied two-disc motion planning by exactly decomposing the con- 
figuration space of each robot, then combining the two solutions to a set of free, 
forbidden and mixed cells, and using PRM to construct the final connectivity 
graph. Zhang et al. [Mj used PRM in conjunction with approximate cell de- 
composition, which also divides space to free, forbidden and mixed cells. Other 
studies have suggested to connect a dense set of near-by configuration space 
"slices" . Each slice is decomposed to free and forbidden cells, but adjacent slices 
are connected in an inexact manner, by e.g., identifying overlaps between adja- 
cent shces [m pp. 283-287], or heuristic interpolation and local-planning ^^. 
In [3S] a 6 dof RRT planner is presented with a 3 dof local planner hybridizing 
probabilistic, heuristic and deterministic methods. 

1.1 Contribution 

In this study, we present a novel general scheme for motion planing via man- 
ifold samples (MMS), which extends sampling-based techniques like PRM as 
follows: Instead of sampling isolated robot configurations, we sample entire low- 
dimensional manifolds, which can be analyzed by complete and exact methods 
for decomposing space. This yields an explicit representation of maximal con- 
nected patches of free configurations on each manifold, and provides a much 
better coverage of the configuration space compared to isolated point samples. 
At the same time, the manifold samples are deliberately chosen such that they 
are likely to intersect each other, which allows to establish connections among 
different manifolds. The general scheme of MMS is illustrated in Figure [TJ A 
detailed discussion of the scheme is presented in Section [2J 

In Section |3J we discuss the application of MMS to the concrete case of a 
polygonal robot translating and rotating in the plane amidst polygonal obsta- 
cles. We present in detail appropriate families of manifolds as well as filtering 
schemes that should also be of interest for other scenarios. Although our software 
is prototypical, we emphasize that the achieved results are due to careful design 
and implementation on all levels. In particular, in Section l4] we present an ex- 
act analytic solution and efficient implementation to a motion planning problem 
instance: moving a polygonal robot in the plane with rotation and translation 
along an arbitrary axis. To the best of our knowledge the problem has not been 
analytically studied before. The implementation involves advanced algebraic and 
extension of state-of-the-art applied geometry tools. In Section [5] we present ex- 
perimental results, which show our method's superior behavior for several test 
cases vis-a-vis a common implementation of the sampling-based PRM algorithm. 
For example, in a tight passage scenario we demonstrate a 27-fold improvement. 
We conclude with a discussion of extensions of our scheme, which we anticipate 
could greatly widen the scope of applicability of sampling-based methods for mo- 
tion planning by combining them with strong analytic tools in a straightforward 
manner. 



2 General Scheme for Planning with Manifold Samples 

Preprocessing — Constructing connectivity graph: We propose a multi- 
query planner for motion planning problems in a possibly high-dimensional con- 
figuration space. The preprocessing stage constructs the connectivity graph of C, 
a data structure that captures the connectivity of C using manifolds as samples. 
The manifolds are decomposed into cells in Cfrce and Cforb in a complete and 
exact manner; we call a cell of the decomposed manifold that lies in Cfrcc a- /ree 
space cell (FSC) and refer to the connectivity graph as G- The FSCs serve as 
nodes in Q while two nodes in Q are connected by an edge if their corresponding 
FSCs intersect. See Figure [l] for an illustration 




Fig. 1: Three-dimensional configuration space: The left side illustrates two fam- 
ilies of manifolds where the decomposed cells are darkly shaded. The right side 
illustrates their intersection that induces the graph Q. 



We formalize the preprocessing stage by considering manifolds induced by a 
family of constraints 'F, such that tp G 'F defines a manifold m^ of the configura- 
tion space. The construction of a manifold m^ and its decomposition into FSCs 
are carried out via a •f'-primitive (denoted P:^) applied to an element tp € ^. By 
a slight abuse of notations we refer to an FSC both as a cell and a node in the 
graph. Using this notation, Algorithm [I] summarizes the construction of Q. In 
lines 3-4, a new manifold constraint is generated and added to the collection of 
manifold constraints X. In lines 5-7, the manifold induced by the new constraint 
is decomposed by the appropriate primitive and its FSCs are added to Q. 

Query: Once the connectivity graph Q has been constructed it can be queried 
for paths between two configurations Qs and qt in the following manner: A man- 
ifold that contains Qs (respectively qt) in one of its FSCs is generated and 
decomposed. Its FSCs and their appropriate edges are added to G- We compute 
a path p in Q between the FSCs that contain qs and qt- A path in Cftcc rnay then 
be computed by planning a path within each FSC in p. 



Algorithm 1 Construct Connectivity Graph 



1: V^0, £^0, X^0 

2: repeat 

3: ?/) <— gcnerate_constraint(l/,-E,X) 

4: X^Xu{ip} 

5: FSCm^, ^ Pi,{m^) 

6: V ^ V U {fscj fscG FSC'n,^ } 

7: E^ EU {(fsci, fsca) | fsci G V, fsc2 G -FS-C™^ , 

fscinfsc2 7^ & fsci /fsC2} 

8: until stopping_condition 

9: return G{V,E) 



2.1 Desirable Properties of Manifold Families 

Choosing the specific set of manifold families may depend on the concrete prob- 
lem at hand, as detailed in the next section. However, it seems desirable to retain 
some general properties. First, each manifold should be simple enough such that 
it is possible to decompose it into free and forbidden cells in a computationally 
effcient manner. The choice of manifold families should also cover the configura- 
tion space, such that each configuration intersects at least a single manifold m^. 
In addition, local transitions between close-by configurations should be made 
possible via cross-connections of several intersecting manifolds, which we term 
the spanning property. We anticipate that these simple and intuitive proper- 
ties (perhaps subject to some fine tuning) may lead to a proof of probabilistic 
completeness of the approach. 

2.2 Exploration and Connection Strategies 

A naive way to generate constraints that induce manifolds is by random sam- 
pling. Primitives may be computationally complex and should thus be applied 
sparingly. We suggest a general exploration/connection scheme and additional 
optimization heuristics that may be used in concrete implementations of the 
proposed general scheme. We describe strategies in general terms, providing con- 
ceptual guidelines for concrete implementations, as demonstrated in Section |3] 

Exploration and connection phases: Generation of constraints is done in 
two phases: exploration and connection. In the exploration phase constraints are 
generated such that primitives will produce FSCs that introduce new connected 
components in Cfree- The aim of the exploration phase is to increase the coverage 
of the configuration space as efficiently as possible. In contrast, in the connection 
phase constraints are generated such that primitives will produce FSCs that 
connect existing connected components in Q. Once a constraint is generated, Q 
is updated as described above. Finally, we note that we can alternate between 
exploration and connection, namely we can decide to further explore after some 
connection work has been performed. 

Region of interest (Rol) : Decomposing an entire manifold m^ by a primi- 
tive Pxp may be unnecessary. Patches of tti^ may intersect Cfree in highly explored 



parts or connect already well-connected parts of Q while others may intersect 
Cfioc in sparsely explored areas or less well connected parts of Q. Identifying 
the regions where the manifold is of good use (depending on the phase) and 
constructing m^, only in those regions increases the effectiveness of P^ while 
desirably biasing the samples. We refer to a manifold patch that is relevant in a 
specific phase as the Region of Interest - Rol of the manifold. 

Constraint filtering Let -0 € '^ be a constraint such that applying P^ 
to ■0 yields the set of FSCs on m^. If we are in the connection phase, insert- 
ing the associated nodes into G and intersecting them with the existing FSCs 
should connect existing connected components of Q. Otherwise, the primitive's 
contribution is poor. We suggest applying a filtering predicate immediately af- 
ter generating a constraint ip to check if Pip (tp) may connect existing connected 
components oi Q. If not, the primitive should not be constructed and ip should 
be discarded. 

3 The Case of Rigid Polygonal Motion 

We demonstrate the scheme suggested in Section [2] by considering a polygonal 
robot R translating and rotating in the plane amidst polygonal obstacles. A 
configuration of R describes the position of the reference point (center of mass) 
of R and the orientation of R. As we consider full rotations, the configuration 
space C is the three dimensional space M^ x 5^. 

3.1 Manifold Families 

As defined in Section [2l we consider manifolds defined by constraints and con- 
struct and decompose them using primitives. We suggest the following con- 
straints restricting motions of R and describe their associated primitives: The 
Angle Constraint fixes the orientation of R while it is still free to translate 
anywhere within the workspace; the Segment Constraint restricts the position 
of the reference point to a segment in the workspace while R is free to rotate. 

The left part of Figure [T] demonstrates decomposed manifolds associated to 
the angle (left bottom) and segment (left top) constraints. The angle constraint 
induces a two-dimensional horizontal plane where the cells are polygons. The 
segment constraint induces a two-dimensional vertical slab where the cells are 
defined by the intersection of rational curves (as explained in Section |4]) . 

We delay the discussion of creating and decomposing manifolds to Section |4j 
For now, notice that the Segment-Primitive is far more time-consuming than the 
Angle-Primitive. 

3.2 Exploration and Connection Strategies 

We use manifolds constructed by the Angle-Primitive for the exploration phase 
and manifolds constructed by the Segment-Primitive for the connection phase. 
Since the Segment-Primitive is far more costly than the Angle-Primitive, we 
focused our efforts on optimizing the former. 



Region of interest - Rol: As suggested in Section 2.2 we may consider the 



Segment-Primitive in a subset of the range of angles. This results in a somewhat 



Algorithm 2 Generate Segment Constraint {V,E) 



1 
2 
3 

4 
5 
6 
7 
8 
9 
10 
11 



if random_nuni ([0, 1]) > random_threshold tlien 

return randoin_segment_procedure() 
else 

fsc <— random_fsc(V) 

a <— [size(fsc) - smalLcelLsize] / [large_celLsize - smalLcelLsize] 

if random_num([0, 1]) > Q then 

return small_cell_procedure(fsc,1/) 

else 

return large_cell_procedure(fsc,V) 

end if 
end if 



"weaker" yet more efficient primitive than considering the whole range. If the 
connectivity of a local area of the configuration space is desired, then using this 
optimization may suffice while considerably speeding up the algorithm. 

Generating segments: Consecutive layers (manifolds of the Angle Con- 
straint) have a similar structure unless topological criticalitics occur in C. Once 
a topological criticality occurs, an FSC either appears and grows or shrinks 
and disappears. We thus suggest a heuristic for generating a segment in the 
workspace for the Segment-Primitive using the size of the cell as a parameter 
where we refer to small and large cells according to pre-defined constants. The 
Rol used will be proportional to the size of the FSC. The segment generated will 
be chosen with one of the following procedures which are used in Algorithm |2] 

Random procedure: Return a random segment from the workspace. 

Large cell procedure: Return a random segment in the cell. 

Small cell procedure: Intersect the FSC with the next (or the previous) 
layer. Return a segment connecting a random point from the FSC and a random 
point in the intersection. 

Constraint Filtering: As suggested in Section |2.2[ we avoid computing 
unnecessary primitives. All FSCs that will intersect a "candidate" constraint 
s, namely all FSCs of layers in its Rol, are tested. If they are all in the same 
connected component in Q, s can be discarded as demonstrated in Algorithm [3J 

3.3 Path Planning Query 

For a query q = {qs,qt), where g^ = {xs,ys,Os) and qt = {xt,yt,Ot), PeiOs) and 
Pe{Qt) are constructed and the FSCs are added to Q. A path of FSCs between the 
FSCs containing qs and qt is searched for. A local path in an Angle-Primitive's 
FSC (which is a polygon) is constructed by computing the shortest path on the 
visibility graph defined by the vertices of the polygon. A local path in an FSC of 
a Segment-Primitive (which is an arrangement cell) is constructed by applying 
cell decomposition and computing the shortest path on the graph induced by 
the decomposed cells. (Figure |4] depicts a path in a segment manifold.) 



Algorithm 3 Filter Segment {s,RoI,V,E) 



i 


CCids ^ V 


2 


for all V £ V do 


3 


fsc <— free_space_cell(t;) 


4 


if constraining_angle(fsc)e Rol then 


5 


CCids •<— cCitjsU connected_coinponent_id(w) 


6 


end if 


7 


end for 


8 


if CCids < 1 then 


9 


return filter_out 


10 


end if 



4 Efficient Implementation of Manifold Decomposition 

The algorithm discussed in Section [3] is implemented in CH — h. It is based on 
Cgal's arrangement package, which is used for the geometric primitives, and 
the Boost graph library [35], which is used to represent the connectivity graph 
Q. We next discuss the manifold decomposition methods in more detail. 

Angle-Primitive: The Angle-Primitive for a constraining angle 9 (denoted 
PeiO)) is constructed by computing the Minkowski sum of —Re with the obsta- 
cle^ The implementation is an application of Cgal's Minkowski sums pack- 
age [331 C.24]. We remark that we ensure (using the method of Canny et al. [7]) 
that the angle 9 is chosen such that sin0 and cos 6* are rational. This allows for 
an exact rotation of the robot and an exact computation of the Minkowski Sum. 

Segment-Primitive: Limiting the possible positions of the robot's reference 
point r to a given segment s, results in a two-dimensional configuration space. 
Each vertex (or edge) of the robot in combination with each edge (or vertex) of 
an obstacle gives rise to a critical curve in this configuration space. Namely the 
set of all configurations that put the two features into contact, and thus mark 
a potential transition between Cforb and Cfree- Our analysis (Appendix pi) shows 
that these critical curves can be expressed by rational functions only. Thus, 
the implementation of the Segment-Primitive is first of all a computation of an 
arrangement of rational functions. 

Cgal follows the generic programming paradigm 2J, that is, algorithms are 
formulated and implemented such that they abstract away from the actual types, 
constructions and predicates. Using the C++ programming language this is re- 
alized by means of class and function templates, respectively. In particular, the 
arrangement package is written such that it takes a traits class as a template 
argument. This traits class defines the supported curve type and provides the 
operations that are required for this type. Since the old specialized traits class 
was too slow (even slower than the solution for general algebraic curves presented 
in [B]), we devised a new efficient traits class for rational functions. 

The new traits class (all details in Appendix IaI) is written such that it takes 
maximal advantage of the fact that the supported curves are functions. As op- 
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(a) Tunnel scenario (b) Snake scenario (c) Flower scenario 

Fig. 2: Experimental scenarios 

posed to the general traits in |Bj, we never have to shear the coordinate sys- 
tem and we only require tools provided by the univariate algebraic kernel of 
Cgal [33l C.8]. A comparison using the benchmark instances that were also 
used in [3] shows that the new traits class is about 3-4 times faster then the 
general traits class; this is a total speed up of about 10 when compared to the 
old dedicated traits class. 

The development of this new traits class represents the low tier of our efforts 
to produce an effective motion planner and relies on a more intimate acquain- 
tance with Cgal in general and arrangement traits for algebraic curves in par- 
ticular; therefore we defer further details to the appendix. We note that the new 
traits class has been accepted for integration into Cgal and will be available in 
the upcoming Cgal release 3.9. 



5 Experimental Results 

We demonstrate the performance of our planner using three different scenarios. 
All scenarios consist of a workspace, a robot with obstacles and one query (source 
and target configurations). Figure [2] illustrates the scenarios where the obstacles 
are drawn in blue and the source and target configurations are drawn in green 
and red, respectively. All reported tests were measured on a Dell 1440 with one 
2.4GHz P8600 Intel Core 2 Duo CPU processor and 3GB of memory running with 
a Windows 7 32-bit OS. Preprocessing times presented are times that yielded at 
least 80% (minimum of 5 runs) success rate in solving queries. 

5.1 Algorithm Properties 

Our planner has two parameters: the number ng of layers to be generated and 
the number Ug of segment constraints to be generated. We chose the following 
values for these parameters: ng g {10,20,40,80} and Ug £ {2^\i e N,i < 14}. 
For a set of parameters (ng, n^) we report the preprocessing time t and whether 
a path was found (marked /) or not found (marked x) once the query was 
issued. The results for the flower scenario are reported in Table[TJ We show that a 
considerable increase in parameters has only a limited effect on the preprocessing 
time. 

In order to test the effectiveness of our optimizations, we ran the planner with 
and without any heuristic for choosing segments and with and without segment 
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filtering. We also added a test with all optimizations using the old traits class. 
The results for the flower scenario can be viewed in Table [21 We remark that the 
engineering work invested in optimizing MMS yielded an algorithm comparable 
and even surpassing a motion planner that is in prevalent use as shown next. 

5.2 Comparison With PRM 

We used an implementation of the PRM algorithm as provided by the OOPSMP 
package [17] . For fair comparison, we did not use cycles in the roadmap as cycles 
increase the preprocessing time significantly. We manually optimized the param- 
eters of each planner over a concrete set. As with previous tests, the parameters 
for MMS are ng and Ug. The parameters used for the PRM are the number 
of neighbors (denoted k) to which each milestone should be connected and the 
percentage of time used to sample new milestones (denoted % st in Table Isl). 

Furthermore, we ran the flower scenario several times, progressively increas- 
ing the robot size. This caused a "tightening" of the passages containing the 
desired path. Figure [3] demonstrates the preprocessing time as a function of the 
tightness of the problem for both planners. A tightness of zero denotes the base 
scenario (Figure [2c]) while a tightness of one denotes the tightest problem solved. 

The results show a speedup for all scenarios when compared to the PRM 
implementation. Moreover, our algorithm has little sensitivity to the tightness 
of the problem as opposed to the PRM algorithm. In the tightest experiment, 
MMS runs 27 times faster than the PRM implementation. 

6 Further Directions 

To conclude, we outline directions for extending and enhancing the current work. 
Our primary goal is to use the MMS framework to solve progressively more com- 
plicated motion-planning problems. As suggested earlier, we see the framework 
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as a platform for convenient transfer of strong geometric primitives into motion 
planning algorithms. For example, among the recently developed tools are effi- 
cient and exact solutions for computing the Minkowski sums of polytopes in M^ 
(see Introduction) as well as for exact update of the sum when the polytopes ro- 
tate [IH] . These could be combined into an MMS for planning full rigid motion of 
a polytope among polytopes, which, extrapolating from the current experiments 
could outperform more simplistic solutions in existence. 

Looking at more intricate problems, we anticipate some difficulty in turning 
constraints into manifolds that can be exactly decomposed. We propose to have 
manifolds where the decomposition yields some approximation of the FSCs, us- 
ing recent advanced meshing tools for example. We can endow the connectivity- 
graph nodes with an attribute describing their approximation quality. One can 
then decide to only look for paths all whose nodes are above a certain approxi- 
mation quality. Alternatively, one can extract any solution path and then refine 
only those portions of the path that are below a certain quality. 

Beyond motion planning: We foresee an extension of the framework to 
other problems that involve high-dimensional arrangements of critical hypersur- 
faces. It is difficult to describe the entire arrangement analytically, but there 
are often situations where constraint manifolds could be computed analytically. 
Hence, it is possible to shed light on problems such as loop closure and assembly 
planning where we can use manifold samples to analytically capture pertinent 
information of high-dimensional arrangements of hypersurfaces. Notice that al- 
though in Section [3] we used only planar manifolds, there are recently developed 
tools to construct two dimensional arrangement of curves on curved surfaces [S] 
which gives further flexibility in choosing the manifold families. 

For supplementary material and updates the reader is referred to our webpage 



http: //acg. cs .tau.ac. il/projects/mms 
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A Traits Class for Rational Functions 



For completeness we repeat below (in Section A.l I material that already appears 



in the body of the paper. More technical details on the implementation of the 



traits class are given in Section A. 2 



A.l Background 

Cgal follows the generic programming paradigm [3^, that is, algorithms are for- 
mulated and implemented such that they abstract away from the actual types, 
constructions, and predicates. Using the C++ programming language this is re- 
alized by means of class and function templates, respectively. The manifold de- 
composition methods described below use Cgal's arrangement package. 

As most other high level packages of Cgal, the arrangement package is 
written such that it takes one traits class as a template argument. This traits 
class defines the type of curves in use and the required operations on them. This 
gives tremendous flexibility in using the package for a variety of different motion 
planning (sub)problems. Here we use it once for line segments via the Minkowski 
sums package written on top of the arrangement package, and once with graphs 
of rational functions, for which we devised a new and particularly efficient traits 
class. The following section gives more detail about the later. 

The new traits class is written such that it takes maximal advantage of the 
fact that the supported curves are functions. This implies that, as opposed to 
the general traits in [6l, we never have to shear the coordinate system and that 
we only require tools provided by the univariate algebraic kernel [331 C.8]. 

A traits class is required to provide (and by that define) the curve type in use. 
For these curves it must provide a specific set of operations, such as splitting 
curves into x-monotone sub-curves, computing the intersections of two curve 
segments, comparing intersection points, or comparing the y-order of two curves 
at a certain x-coordinate. Thus, our traits class implements these predefined 
operations explicitly for rational functions. The heart of the traits consists of two 
classes that represent the complete topology of one or two functions, respectively. 
All predicates and constructions required for the traits are essentially just queries 
to one of these two classes. 

A. 2 Technical Details 

For one function f{x) = fn{x)/fd{x), with fn,.fd G "^[X] coprime, we subdivide 
the a;-axis into intervals such that the sign of / is invariant within each interval. 
This is obtained by computing the sorted sequence of the real roots (with mul- 
tiplicity) of /„ and fd- For the right-most interval the sign is determined by the 
signs of the leading coefficients of /„ and fd- The remaining signs are concluded 
from right to left using the multiplicity of the computed roots. 

For two functions f{x) = fn{x)/ fd{x) and g{x) = gn{x)/gd{x), we similarly 
subdivide the x-axis into intervals such that the y-order of / and g is invariant 
within each interval. Of course, the order may change at intersection points, the 



x-coordinates of which are given by the real roots of r — fn9d — 9nfd G Z[X]. 
However, the order may also change at vertical asymptotes of / and g. Thus, 
the subdivision is given by the sorted sequence of the real roots of fj^, g^ and r. 
Again, once the order is computed for one interval, the others can be concluded 
via the multiplicities of the roots. All results are cached such that each instance 
of both classes is computed at most once. 

As stated in Section |4] comparison with the benchmark instances that were 
also used in ^ shows that the new traits class is about 3-4 times faster then 
the general traits class, this is a total speed up of about 10 when compared to 
the old dedicated traits class, which was based on Core. We remark that the 
new traits class has already been accepted for integration into Cgal and will be 
available in the upcoming Cgal release 3.9. 

B Critical Curves of a Rotating Robot Along a 
Translation Segment 



B.l The problem 

We consider a polygonal robot translating along a fixed segment while rotating 
amidst polygonal obstacles. This means that the reference point of the robot 
moves along a fixed line segment, and the robot can rotate around this reference 
point. A critical curve is a curve representing a motion of the robot while a 
feature of the robot is in contact with a feature of an obstacle: Either a robot's 
vertex is in contact with an obstacle's edge or a robot's edge is in contact with an 
obstacle's vertex. These cases will be referred to as vertex-edge and edge-vertex 
respectively. Figure |4] demonstrates part of an arrangement of critical curves 
constructed for the Tunnel scenario (Figure 2a I for a segment connecting the 



source and target configurations. The green and red crosses mark the source and 
target configurations respectively and the blue poly-line is a path constructed 
within an FSC. 




Fig. 4: Arrangement of critical curves 



We define the critical curves by first assuming that both the segment and the 
edge at hand (either the robot's or the obstacle's) is a full line (containing the 
edge). We then introduce "critical endpoints" taking into account the fact that 



the robot's translation is restricted to a segment and that the edge considered is 
not a hne. This is done by identifying the geometric locus where a vertex and an 
edge's endpoint are in contact. We omit this additional stage from the appendix 
though we addressed it in our work. 

B.2 Robot representation and definitions 

A robot i? is a simple polygon with vertices {vi, . . . , Vn} where Vi = {xi, yi)^ and 
edges {(t'i,W2), . . . (wn,wi)}. We assume that the reference point of R is located 
at the origin. The position of R in the workspace is defined by a configuration 
q = irq,0q) where r, = (x,, y,)^. 

Thus, q maps the position of a vertex Vi by the following parameterization: 



V,{q) = M{6q)Vi +rq 



where M{6) = 



is the rotation matrix. 



cos W — sm ( 
sin 6 cos 9 



Given a segment seg = [s,t\ where s = {xs,ys) ^-^d t = {xt,yt) ^^ define 
the parametrization (a, r) S [0, 1] x M in Equations (fTl), ([2| 

Vq = (1 - a)s + at, (1) 



9q = 2 arctan r. (2) 

The parametrization fixes the robot's reference point to the supporting line 
of seg. The parametrized vertex is represented in Equation ([3]) 



Vi{a,T) — M{T)vi + {I — a)s + at, (3) 

where M(t) = jr^ 



1-t2 -2t 
2t 1 - r2 



B.3 Robot's vertex - Obstacle's edge 

Let Vi be a robot's vertex and e be an obstacle's edge that is supported by the 
line I : ax + by + c = 0. The critical curve in this case is defined by adding the 
constraint that Vi{q) S I thus: 

ax^{q) + by^{q)+c^O. (4) 

Plugging Equation ^ into Equation Q yields: 

a[(l - T'^)xi - 2Tyi] + a{l + t'^){1 - a)xs + a{l + T'^)axt+ 

b[{2T)x, + (1 - T^)y,] + 6(1 + t2)(1 - a)ys + 6(1 + T^)ayt+ 

c(1 + t2)=0, 

when simplified: 

T^ • [-axi + axs - byi + bys + c] + 



r • [^2ayi + 2bxi]+ 1 • [axi + aXs + hyi + hyg + c] + 

q;t^ • [— aXg + axt — byg + 6yt]+ a ■ [—aXg + axj — byg + fey^] = 0. 

Hence: 

PaT^+PiT+po .^x 

"= ^, > (5) 

q2T^ + go 

where 

P2 = a{Xi - Xg) + b{y, - y^) - c, ^2 = a(xt - Xg) + b{yt - yg), 

Pi =2{ayi-bxi), 

Po ^ -a{xi+Xg)-b{yi+yg)-c, qo = q2- 



B.4 Robot's edge - Obstacle's vertex 

Let e = (wi,W2) be a robot's edge and vq be an obstacle's vertex. The critical 
curve in this case is defined by adding the constraint that vq lies on the line 
l^ : ax + by + c = supporting e. To simplify the notation we will consider 
I : {1 + T^)ax + (1 + T^)by + (1 + t^)c — and the constraint that i;o G I. 
This line has the following coefficients: (1 + T'^)a = (1 + t^)(j/2('7) — yi{q)), 
(l+r2)6 = (l+r2)(xi((z)-X2(<z)) and (l+r2)c = il+T^){x2{q)yi{q)-xi{q)y2{q)). 
Let us denote A^ = {x2 — xi) and Ay — {y2 — Hi)- 

Simplifying the coefficients of I yields: 



(l + r2)a=(l + r2)(y2(q)-yi(g)) 

= [{2t)x2 + (1 - T^)V2\ + (1 + T^)[{1 - a)ys + ayt] 
-[{2t)xi + (1 - T^)yi] - (1 + r^m - a)ys + ayt] 

= [Ay + {2A,)t - AyT^], 

{1 + T^)b = {I + r^)(x-,(q) ~ X2{q)) 

= [(1 - T^)xi - 2ryi] + (1 + r2)[(l - a)x, + axt] 

-[(1 - T^)X2 - 2ry2l - (1 + T^)[(l - a)xs + axt] 
= [-Aa, + Ay2T + A^T^], 

(1 + T^)c = (1 + T^){x2{q)yi{q) - xi(q)y2{q)) 

= (1 + T^)[j^ [(1 - T^)X2 - 2ry2] + (1 - a)xs + axt] 
[j^[{2t)x^ + (1 - T^)yi] + (1 - a)y. + ayt] 
-(1 + ^^)[iq^[(l - r'')xi - 2ryi] + (1 - a)xs + Qa;*] 
[j:^[{2t)x2 + (1 - r2)jy2] + (1 - a)y, + ayt] 

= (1+^2) [-4(a:it/2 - a;2j/i)r^ - (2:11/2 - X2yi){l - r^)^] 
+[(1 - T^)A^ - 2rAy][ys + (yi - ys)a] 
+ [-2tA^ - (1 - T^)'4j,][a;s + {xt - Xs)a] 



(1 + t'^)c = -(1 + r^)(a;ij/2 - X2yi) 

+{A^ - 2AyT - Aa,T^){ys + (j/t - ys)a) 
+(-Z\y - 2A^T + AyT'^)(x, + {xt - Xs)c 



Denoting k — Xiy2 — X22/1 and inserting vq into the line equation of fyields: 
[Ay + {2A^)t - AyT^]xo +[-A^ + Ay2T + A^T^]yo -fc(l + r^) 
+ [A^-2AyT-A^T^][y, + {yt''ys)a] +[-Ay-2A^T+AyT^][x, + {xt-Xs)a] = 0. 

Now, 

r^ ■ ['4a:(yo - Vs) - Ayixo - Xs) - k] + 

T ■ [2A^{xo - Xs) + 2Ay{yn - ys)]+ 1 • [-A^iyn - y^) + Ay{xo - x^) - k] + 

ar'^ ■ [-A^iyt - ys) + Ay{xt - Xs)] + 

ar ■ [-2Ay{yt - ys) - 2A^(xt - Xs)\-\- a ■ [Ax{yt - ys) - Ay{xt - Xs)] = 0. 



Finally: 



where 



TO2T^ + miT + mo 
a == ^ , (d) 

n2r^ + TiiT + no 



m2 = Ay{xo-Xs)-A^{yo~ys) + k, na = Ay{xt - Xs) - A^{yt - ys), 

mi = -2Ax{xo-Xs)-2Ay{yo-ys), ni = -2Ax{xt - Xs) - 2Ay{yt - ys), 

mo = — ?7i2 + 2k, no = — ^2 and 

Aa: = {X2 - Xi) , Ay = (y2 - J/l ) , fc = Xiy2 - X2yi. 



